#include "HermiteInterpolation.hpp"
#include <tuple>
#include <algorithm>
#include <numeric>


HermiteInterpolation::HermiteInterpolation(const std::vector<double>& x, const std::vector<double>& y, const std::vector<int>& exp) : x(x), y(y) {
    if (x.size() != y.size() ) {
        throw std::invalid_argument("The sizes of x, y must be equal.");
    }

    int n = x.size();
    std::vector<std::tuple<double, double, int>> data;
    for (int i = 0; i < n; i++)
    {
        data.push_back(std::make_tuple(x[i], y[i], exp[i]));
    }

    std::sort(data.begin(), data.end(), [](const std::tuple<double, double, int> &a, const std::tuple<double, double, int> &b) -> bool{
        if (std::abs(std::get<0>(a) - std::get<0>(b)) < 1e-16)
        {
            return std::get<2>(a) < std::get<2>(b);
        }
        return std::get<0>(a) < std::get<0>(b);
    });

    for (int i=0; i<n; i++)
    {
        this->x[i] = std::get<0>(data[i]);
    }

    std::vector<std::vector<double>> divided_diff;
    divided_diff.resize(n);

    for (int i = 0; i < n; i++)
    {
        divided_diff[i].resize(i + 1, nan("")); /**set the unknown to nan */
        int index = i-std::get<2>(data[i]);
        divided_diff[i][0] = std::get<1>(data[index]); 
        double divisor = 1.0;
        for (int j = 1; j<=std::get<2>(data[i]); j++)
        {
            divided_diff[i][j] = std::get<1>(data[index+j]) / divisor;
            divisor *= (j+1);
        }
    }

    for (int j = 1; j < n; j++)
    {
        for (int i = j; i < n; i++)
        {
            if (std::isnan(divided_diff[i][j]))
            {
                divided_diff[i][j] = (divided_diff[i][j - 1] - divided_diff[i - 1][j - 1]) / (std::get<0>(data[i]) - std::get<0>(data[i - j]));
            }
            else
            {
                continue;
            }
        }
    }
    for (int i = 0; i < n; i++)
    {
        z.push_back(divided_diff[i][i]);
    }
}

double HermiteInterpolation::operator()(double eval_x) const {
    int n = x.size();
    double result = 0;

    for (int i = n-1; i >=0; --i) {
        result = result*(eval_x-x[i]) + z[i];
        
    }
    return result;
}

double HermiteInterpolation::derivative(double eval_x) const {
    double epison = 1e-6;
    return (this->operator()(eval_x+epison) - this->operator()(eval_x-epison)) / (2*epison);
}
